{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "92eccd63",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 13: Image Formation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f30b0251",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import google.colab\n",
    "    print('Running on CoLab')\n",
    "    !pip install matplotlib\n",
    "    !pip install machinevision-toolbox-python\n",
    "    COLAB = True\n",
    "except:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "\n",
    "import numpy as np\n",
    "from scipy import linalg\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from machinevisiontoolbox.base import *\n",
    "from machinevisiontoolbox import *\n",
    "from spatialmath.base import *\n",
    "from spatialmath import *\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "8dd03d91",
   "metadata": {},
   "source": [
    "# 13.1 Perspective Camera\n",
    "## 13.1.2 Modeling a Perspective Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "60299c89",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera(f=0.015);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "564d17a9",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = [0.3, 0.4, 3.0];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "293c8748",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e994f4ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point(P, pose=SE3.Tx(-0.5))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3b0d8a62",
   "metadata": {},
   "source": [
    "## 13.1.3 Discrete Image Plane\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6237b7f",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera(f=0.015, rho=10e-6,\n",
    " imagesize=[1280, 1024], pp=[640, 512], name=\"mycamera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "381561e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b599973d",
   "metadata": {},
   "source": [
    "## 13.1.4 Camera Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f1d8fe1",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.K"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3346b186",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.C()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9294c333",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.rad2deg(camera.fov())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad964771",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = np.column_stack([[0, 0, 10], [10, 10, 10]])\n",
    "p, visible = camera.project_point(P, visibility=True)\n",
    "visible"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3c7f80a2",
   "metadata": {},
   "source": [
    "## 13.1.5 Projecting Points\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "170597fa",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = mkgrid(n=3, side=0.2, pose=SE3.Tz(1.0));\n",
    "P.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fbc5d27c",
   "metadata": {},
   "outputs": [],
   "source": [
    "P[:, :4]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8a69b710",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29163156",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_point(P);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "94f4b515",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_camera = SE3.Trans(-1, 0, 0.5) * SE3.Ry(0.9);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1636340b",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_point(P, pose=T_camera);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "52839a72",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point([1, 0, 0, 0], pose=T_camera)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "360c8e28",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera.plot_point(P, pose=T_camera)\n",
    "p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c43f03ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "cube = mkcube(0.2, pose=SE3.Tz(1));\n",
    "cube.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "209b932e",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_point(cube);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "555b2338",
   "metadata": {},
   "outputs": [],
   "source": [
    "X, Y, Z = mkcube(0.2, pose=SE3.Tz(1), edge=True)\n",
    "X.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a61b2723",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_wireframe(X, Y, Z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "caa95165",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_wireframe(X, Y, Z, pose=T_camera);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63f099e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "X, Y, Z = mkcube(0.2, edge=True)\n",
    "for theta in np.linspace(0, 2 * pi, 10):\n",
    "  T_cube = SE3.Tz(1.5) * SE3.RPY(theta * np.array([1.1, 1.2, 1.3]))\n",
    "  camera.clf()\n",
    "  camera.plot_wireframe(X, Y, Z, objpose=T_cube)\n",
    "  plt.pause(0.1)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "0c29840c",
   "metadata": {},
   "source": [
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "This style of animation works well for a Python script.  However, using Jupyter it produces a set of separate images.  The number of frames shown here has been reduced to 10, from 100 as per the in-book example."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9f2b106",
   "metadata": {},
   "source": [
    "## 13.1.6 Lens Distortion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "80e7a0bd",
   "metadata": {},
   "outputs": [],
   "source": [
    "# camera = CentralCamera(f=0.015, rho=10e-6,\n",
    "#   imagesize=[1280, 1024], pp=[512, 512],\n",
    "#   distortion=[k1, k2, k3, p1, p2])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c733da19",
   "metadata": {},
   "source": [
    "# 13.2 Camera Calibration\n",
    "## 13.2.1 Calibrating with a 3D Target\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6f59b02",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = mkcube(0.2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d6336775",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_unknown = SE3.Trans(0.1, 0.2, 1.5) * SE3.RPY(0.1, 0.2, 0.3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4607ea07",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_unknown = CentralCamera(f=0.015, rho=10e-6, imagesize=[1280, 1024], noise=0.05, seed=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3c46acc4",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera_unknown.project_point(P, objpose=T_unknown);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "763f1eee",
   "metadata": {},
   "outputs": [],
   "source": [
    "C, resid = CentralCamera.points2C(P, p)\n",
    "C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1e401ae2",
   "metadata": {},
   "outputs": [],
   "source": [
    "resid"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6f67c18d",
   "metadata": {},
   "source": [
    "## 13.2.2 Calibrating with a Checkerboard\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f30558ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "images = ImageCollection(\"calibration/*.jpg\");\n",
    "images[13].disp()  # display image12\n",
    "len(images)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "09c28e56",
   "metadata": {},
   "outputs": [],
   "source": [
    "K, distortion, frames = CentralCamera.images2C(images, gridshape=(7,6), squaresize=25e-3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "646bbf61",
   "metadata": {},
   "outputs": [],
   "source": [
    "K"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4cc15370",
   "metadata": {},
   "outputs": [],
   "source": [
    "images[0].centre"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "48e8c3c3",
   "metadata": {},
   "outputs": [],
   "source": [
    "for frame in frames:\n",
    "  CentralCamera.plot(pose=frame.pose, scale=0.05)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af8b465e",
   "metadata": {},
   "outputs": [],
   "source": [
    "distortion"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e630ab6b",
   "metadata": {},
   "source": [
    "### 13.2.2.1 Correcting for Lens Distortion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a83a749",
   "metadata": {},
   "outputs": [],
   "source": [
    "u0 = K[0, 2]; v0 = K[1, 2]; fpix_u = K[0, 0]; fpix_v = K[1,1];\n",
    "k1, k2, p1, p2, k3 = distortion;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72f3f95d",
   "metadata": {},
   "outputs": [],
   "source": [
    "U, V = images[12].meshgrid()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ebc4581",
   "metadata": {},
   "outputs": [],
   "source": [
    "u = (U - u0) / fpix_u;\n",
    "v = (V - v0) / fpix_v;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ae94526",
   "metadata": {},
   "outputs": [],
   "source": [
    "r = np.sqrt(u**2 + v**2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c505276",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_u = u * (k1*r**2 + k2*r**4 + k3*r**6) + p1*u*v + p2*(r**2 + 2*u**2);\n",
    "delta_v = v * (k1*r**2 + k2*r**4 + k3*r**6) + p1*(r**2 + 2*v**2) + p2*u*v;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a96f5c33",
   "metadata": {},
   "outputs": [],
   "source": [
    "ud = u + delta_u; vd = v + delta_v;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d86a9af8",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ud = ud * fpix_u + u0;\n",
    "Vd = vd * fpix_v + v0;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c7d4188",
   "metadata": {},
   "outputs": [],
   "source": [
    "undistorted = images[12].warp(Ud, Vd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d842ca3",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()   # clear 3D plot\n",
    "plt.quiver(Ud[::50, ::50], Vd[::50, ::50], -delta_u[::50, ::50], -delta_v[::50, ::50]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "203a6e23",
   "metadata": {},
   "outputs": [],
   "source": [
    "magnitude = np.sqrt(delta_u**2 + delta_v**2);\n",
    "plt.contour(U, V, magnitude);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8f5ac112",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ## 13.2.3 Decomposing the Camera Calibration Matrix\n",
    "o = linalg.null_space(C);\n",
    "o.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d7d8d20",
   "metadata": {},
   "outputs": [],
   "source": [
    "h2e(o).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ddc7a65b",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_unknown.inv().t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "03c2d83e",
   "metadata": {},
   "outputs": [],
   "source": [
    "est = CentralCamera.decomposeC(C)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d128d7ce",
   "metadata": {},
   "outputs": [],
   "source": [
    "est.f / est.rho[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "88eb19f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.f / camera.rho[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b178665",
   "metadata": {},
   "outputs": [],
   "source": [
    "(T_unknown * est.pose).printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0cc99540",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3([-0.9, 0.9, -0.9, 0.9, -1.5, 0.3]);\n",
    "plot_sphere(0.03, P, color=\"r\");\n",
    "SE3().plot(frame=\"T\", color=\"b\", length=0.3);\n",
    "est.plot(scale=0.3, color=\"black\", frame=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88e4ae27",
   "metadata": {},
   "source": [
    "## 13.2.4 Pose Estimation with a Calibrated Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99e5bab9",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_calib = CentralCamera.Default(noise=0.1, seed=0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb2233c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = mkcube(0.2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c731652",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_unknown = SE3.Trans(0.1, 0.2, 1.5) * SE3.RPY(0.1, 0.2, 0.3);\n",
    "T_unknown.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "020f8d61",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera_calib.project_point(P, objpose=T_unknown);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36b270f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_est = camera_calib.estpose(P, p).printline()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17856b9b",
   "metadata": {},
   "source": [
    "# 13.3 Wide Field-of-View Cameras\n",
    "## 13.3.1 Fisheye Lens Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5bedd9cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = FishEyeCamera(\n",
    "          projection=\"equiangular\",\n",
    "          rho=10e-6,\n",
    "          imagesize=[1280, 1024]\n",
    "          );"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02763498",
   "metadata": {},
   "outputs": [],
   "source": [
    "X, Y, Z = mkcube(side=1, centre=[1, 1, 0.8], edge=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "945ca9ef",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_wireframe(X, Y, Z, color=\"k\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "53b9e3e3",
   "metadata": {},
   "source": [
    "## 13.3.2 Catadioptric Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad7e8cc5",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CatadioptricCamera(\n",
    "          projection=\"equiangular\",\n",
    "          rho=10e-6,\n",
    "          imagesize=[1280, 1024],\n",
    "          maxangle=pi/4\n",
    "      );"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ac65b25",
   "metadata": {},
   "outputs": [],
   "source": [
    "X, Y, Z = mkcube(1, centre=[1, 1, 0.8], edge=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "da5d189e",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_wireframe(X, Y, Z, color=\"k\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e415853a",
   "metadata": {},
   "source": [
    "## 13.3.3 Spherical Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a942e6e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = SphericalCamera()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f6a21e5f",
   "metadata": {},
   "outputs": [],
   "source": [
    "X, Y, Z = mkcube(1, centre=[2, 3, 1], edge=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f298b756",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_wireframe(X, Y, Z, color=\"k\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec927ed5",
   "metadata": {},
   "source": [
    "# 13.4 Unified Imaging Model\n",
    "## 13.4.1 Mapping Wide-Angle Images to the Sphere\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "75831a14",
   "metadata": {},
   "outputs": [],
   "source": [
    "u0 = 528.1214; v0 = 384.0784; l = 2.7899; m = 996.4617;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63eadcdc",
   "metadata": {},
   "outputs": [],
   "source": [
    "fisheye = Image.Read(\"fisheye_target.png\", dtype=\"float\", mono=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f9dd6be8",
   "metadata": {},
   "outputs": [],
   "source": [
    "n = 500;\n",
    "phi_range = np.linspace(-pi, pi, n);  # longitude\n",
    "theta_range = np.linspace(0, pi, n);     # colatitude\n",
    "Phi, Theta = np.meshgrid(phi_range, theta_range);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "48cafd80",
   "metadata": {},
   "outputs": [],
   "source": [
    "r = (l + m) * np.sin(Theta) / (l - np.cos(Theta));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8479a241",
   "metadata": {},
   "outputs": [],
   "source": [
    "U = r * np.cos(Phi) + u0;\n",
    "V = r * np.sin(Phi) + v0;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f34d4ff",
   "metadata": {},
   "outputs": [],
   "source": [
    "spherical = fisheye.warp(U, V, domain=(phi_range, theta_range))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e9b6917d",
   "metadata": {},
   "outputs": [],
   "source": [
    "spherical.disp(axes=(\"$\\phi$\", \"$\\theta$\"));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d367cf67",
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = plotvol3();\n",
    "plot_sphere(radius=1, ax=ax, filled=True, resolution=500,\n",
    "  facecolors=spherical.colorize().A, cstride=1, rstride=1);\n",
    "ax.view_init(azim=-143.0, elev=-9)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "42b58f65",
   "metadata": {},
   "source": [
    "## 13.4.2 Mapping from the Sphere to a Perspective Image\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bbf4898c",
   "metadata": {},
   "outputs": [],
   "source": [
    "W = 1000;\n",
    "m = W / 2 / np.tan(np.deg2rad(45 / 2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f37fa2a",
   "metadata": {},
   "outputs": [],
   "source": [
    "l = 0;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "add7512e",
   "metadata": {},
   "outputs": [],
   "source": [
    "u0, v0 = W / 2, W / 2;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c97a5de0",
   "metadata": {},
   "outputs": [],
   "source": [
    "U, V = Image.meshgrid(width=W, height=W);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "542b5949",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(W)\n",
    "print(U.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32aec1dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "U0 = U - u0; V0 = V - v0;\n",
    "r = np.sqrt(U0**2 + V0**2);\n",
    "phi = np.arctan2(V0, U0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6053a2a5",
   "metadata": {},
   "outputs": [],
   "source": [
    "Phi = phi;\n",
    "Theta = pi - np.arctan(r / m);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3952d045",
   "metadata": {},
   "outputs": [],
   "source": [
    "spherical.warp(Phi, Theta).disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a1bc030",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(spherical)\n",
    "print(Phi.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "65dcc264",
   "metadata": {},
   "outputs": [],
   "source": [
    "spherical2 = spherical.rotate_spherical(SO3.Ry(0.9) * SO3.Rz(-1.5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08f462d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "spherical2.warp(Phi, Theta).disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "799b3b75",
   "metadata": {},
   "source": [
    "# 13.6 Applications\n",
    "## 13.6.1 Fiducial Markers\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bf376a81",
   "metadata": {},
   "outputs": [],
   "source": [
    "scene = Image.Read(\"lab-scene.png\", rgb=False)\n",
    "scene.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f96cdb6",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera(f=3045, imagesize=scene.shape, \n",
    "                       pp=(2016, 1512), rho=1.4e-6);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0e218268",
   "metadata": {},
   "outputs": [],
   "source": [
    "markers = scene.fiducial(dict=\"4x4_50\", K=camera.K, side=67e-3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c5834143",
   "metadata": {},
   "outputs": [],
   "source": [
    "markers[2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3a81e6a",
   "metadata": {},
   "outputs": [],
   "source": [
    "markers[2].corners"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5912ffc0",
   "metadata": {},
   "outputs": [],
   "source": [
    "for marker in markers:\n",
    "  marker.draw(scene, length=0.10, thick=20)\n",
    "scene.disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f5739433",
   "metadata": {},
   "source": [
    "## 13.6.2 Planar Homography\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25c6836f",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_camera = SE3.Tz(8) * SE3.Rx(-2.8);\n",
    "camera = CentralCamera.Default(f=0.012, pose=T_camera);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a096b6b",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = np.column_stack([[-1, 1], [-1, 2], [2, 2], [2, 1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "55bfb017",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.project_point(np.vstack([P, np.zeros((4,))]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "68faf4f6",
   "metadata": {},
   "outputs": [],
   "source": [
    "H = np.delete(camera.C(), 2, axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0dffafdc",
   "metadata": {},
   "outputs": [],
   "source": [
    "homtrans(H, P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "61230a22",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = np.column_stack([[0, 0], [0, 1000], [1000, 1000], [1000, 0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "52e36ddc",
   "metadata": {},
   "outputs": [],
   "source": [
    "Pi = homtrans(np.linalg.inv(H), p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59f38598",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot(scale=2, color=\"black\");\n",
    "plot_sphere(radius=0.1, centre=np.vstack((P, np.zeros((4,)))), color=\"red\");\n",
    "plot_sphere(radius=0.1, centre=np.vstack((Pi, np.zeros((4,)))), color=\"blue\");\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4da897c3",
   "metadata": {},
   "source": [
    "# 13.7 Advanced Topics\n",
    "## 13.7.1 Projecting 3D Lines and Quadrics\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "021a9af2",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = Line3.Join((0, 0, 1), (1, 1, 1))\n",
    "L"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ef9ffdcf",
   "metadata": {},
   "outputs": [],
   "source": [
    "L.w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "87bad2b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default();\n",
    "l = camera.project_line(L)\n",
    "l"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f5e4f43d",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_line2(l)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ed52882b",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.plot_line3(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "061554ed",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_camera = pose=SE3.Trans(0.2, 0.1, -5) * SE3.Rx(0.2);\n",
    "camera = CentralCamera.Default(f=0.015, pose=T_camera);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "276b0e65",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = np.diag([1, 1, 1, -1]);\n",
    "Q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27f1e2d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "adj = lambda A: np.linalg.det(A) * np.linalg.inv(A);\n",
    "C = camera.C();\n",
    "c = adj(C @ adj(Q) @ C.T)\n",
    "c"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f37f3d81",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(c[:2, :2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39272863",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "from sympy import symbols, Matrix, Eq, plot_implicit\n",
    "x, y = symbols(\"x y\")\n",
    "X = Matrix([[x, y, 1]]);\n",
    "ellipse = X * Matrix(c) * X.T;\n",
    "plot_implicit(Eq(ellipse[0], 1), (x, 0, 1_000), (y, 0, 1_000), );"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
